/* 
 * Modbus slave mode data control.
 * Copyright (c) 2012  apleilx
 * All rights reserved.
 * no limite
 */

#define _MBDATACB_MODULE_
#include "mbdatacb.h"
#include "mbinputdatacfg.h"

#if MB_REG_INPUT_CB_ENABLE >0 
uint8_t eMBRegInputCB( uint8_t * pucRegBuffer, uint16_t usAddress, uint16_t usNRegs )
{
    uint16_t             iRegIndex;
    mbU16Type           Data;
    
#if REG_INPUT_START == 0
    if( ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) )
    {
#else
    if( ( usAddress >= REG_INPUT_START )&& ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) )
    {
#endif        
        iRegIndex = ( int )( usAddress - REG_INPUT_START );
        while( usNRegs > 0 )
        {
            Data.Val        = RegInput[iRegIndex ++].uVal;
            *pucRegBuffer++ = Data.byte.HB;
            *pucRegBuffer++ = Data.byte.LB;
            usNRegs--;
        }
    }
    else
        return MB_ERR_NOREG;
    return MB_ERR_NONE;    
}

#endif

#if MB_REG_HOLDING_CB_ENABLE > 0
uint8_t eMBRegHoldingCB( uint8_t  * pucRegBuffer, uint16_t usAddress, uint16_t usNRegs, mbRegModeType eMode )
{
    uint16_t             iRegIndex;
    mbU16Type           Data;
    
#if REG_HOLDING_START == 0
    if( ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
    {
#else
    if( ( usAddress >= REG_HOLDING_START ) && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
    {
#endif        
        iRegIndex = ( usAddress - REG_HOLDING_START );
        switch ( eMode )
        {
            /* Pass current register values to the protocol stack. */
            case MB_REG_READ:
                while( usNRegs > 0 )
                {
                    Data.Val        = RegHolding[iRegIndex ++];
                    *pucRegBuffer++ = Data.byte.HB;
                    *pucRegBuffer++ = Data.byte.LB;
                    usNRegs--;
                }
                break;

            case MB_REG_WRITE:
                while( usNRegs > 0 )
                {
                    Data.byte.HB   = *pucRegBuffer++;
                    Data.byte.LB   = *pucRegBuffer++;
                    RegHolding[iRegIndex++] = Data.Val;
                    usNRegs--;
                }
        }
    }
    else
        return MB_ERR_NOREG;
    return MB_ERR_NONE;    
}

#endif

#if MB_REG_COILS_CB_ENABLE > 0
uint8_t eMBRegCoilsCB( uint8_t  * pucRegBuffer, uint16_t usAddress, uint16_t usNCoils, mbRegModeType eMode )
{

	mbU16Type          DataWR;
    uint16_t          iRegIndex;
	uint8_t           BitOffset;
	uint8_t           ReadMask;
	mbU16Type          WriteMask;
	
#if REG_COILS_START == 0
    if( ( usAddress + usNCoils <= REG_COILS_START + REG_COILS_NREGS * 8 ) )
    {
#else
    if( ( usAddress >= REG_COILS_START ) && ( usAddress + usNCoils <= REG_COILS_START + REG_COILS_NREGS * 8 ) )
    {
#endif
		BitOffset = usAddress % 8;                                
        iRegIndex = (usAddress - REG_COILS_START) / 8;             
		
        switch ( eMode )
        {
            /* Pass current register values to the protocol stack. */
            case MB_REG_READ:
                DataWR.byte.HB   = RegCoil[iRegIndex ++];     
                DataWR.Val     >>= (BitOffset);                       
                while( usNCoils > 0 )
                {
                    DataWR.Val >>= (8 - BitOffset);	                    
                    DataWR.byte.HB = RegCoil[iRegIndex ++];     
                    DataWR.Val >>= (BitOffset);                        
                    if(usNCoils < 8)                                
                    {
                        ReadMask          = 0xFF;
                        DataWR.byte.LB &= ~( ReadMask << usNCoils );   
                        usNCoils = 0;
                    }
                    else
                        usNCoils-=8;
                    *pucRegBuffer++ = DataWR.byte.LB;
                }
                break;

            /* Update current register values with new values from the
             * protocol stack. */
            case MB_REG_WRITE:
                while( usNCoils > 0 )
                {
                    DataWR.byte.LB = *pucRegBuffer++;             
                    DataWR.Val   <<= BitOffset;                    
                    WriteMask.Val= 0xFF;                                   
                    if(usNCoils < 8)                                 
                    {
                        WriteMask.Val >>= (8 - usNCoils);               
                        usNCoils = 0;
                    }
                    else
                        usNCoils -= 8;
                    WriteMask.Val   <<= BitOffset;                  
				
                    DataWR.Val   &= WriteMask.Val;	                 
                    RegCoil[iRegIndex]    &=  ~WriteMask.byte.LB;  
					RegCoil[iRegIndex+1]  &=  ~WriteMask.byte.HB;		
                    RegCoil[iRegIndex]    |=  DataWR.byte.LB;  
					RegCoil[iRegIndex+1]  |=  DataWR.byte.HB;	
					iRegIndex++;					
                }
        }
    }
    else
        return MB_ERR_NOREG;
    return MB_ERR_NONE;    
}

#endif

#if MB_REG_DISCRETE_CB_ENABLE > 0
uint8_t eMBRegDiscreteCB( uint8_t  *pucRegBuffer, uint16_t usAddress, uint16_t usNCoils )
{

	mbU16Type          DataWR;
    uint16_t          iRegIndex;
	uint8_t           BitOffset;
	uint8_t           ReadMask;

#if REG_DISCRETE_START == 0
    if( ( usAddress + usNCoils <= REG_DISCRETE_START + REG_DISCRETE_NREGS * 8 ) )
    {
#else
    if( ( usAddress >= REG_DISCRETE_START ) && ( usAddress + usNCoils <= REG_DISCRETE_START + REG_DISCRETE_NREGS * 8 ) )
    {
#endif        
		BitOffset = usAddress % 8;                                 
        iRegIndex = (usAddress - REG_DISCRETE_START) / 8;             
		
		DataWR.byte.HB   = RegDiscrete[iRegIndex ++];      
		DataWR.Val     >>= (BitOffset);                      
        while( usNCoils > 0 )
        {
			DataWR.Val >>= (8 - BitOffset);	                   
			DataWR.byte.HB = RegDiscrete[iRegIndex ++];    
			DataWR.Val >>= (BitOffset);                     
			if(usNCoils < 8)                               
			{
				ReadMask          = 0xFF;
				DataWR.byte.LB &= ~( ReadMask << usNCoils );   
				usNCoils = 0;
			}
			else
				usNCoils-=8;
            *pucRegBuffer++ = DataWR.byte.LB;
        }
	}
    else
        return MB_ERR_NOREG;
    return MB_ERR_NONE;
}

#endif


